#include "INS570D/INS570D_Driver.h"

INS570D_Driver::INS570D_Driver(size_t size_):
    BufferParaseInterface(),
    buffer_size_(size_)
{
    //buffer_ = new unsigned char[buffer_size_];
    buffer_.reserve(buffer_size_);
}

INS570D_Driver::~INS570D_Driver()
{
    //delete[] buffer_;
    buffer_.clear();
}

void INS570D_Driver::parse()
{
    dummy_array_.clear();
    std::vector<uint32_t> dummy_array(25);
    for (int i=0;i<25;i++) {
        dummy_array[i]=getOneValue<uint32_t>(config_[i][0],config_[i][1]);
        //printf("%x\n",dummy_array[i]);
    }
    dummy_array_=dummy_array;
    //printf("%x\n",dummy_array_[0]);

    INS_STATUS test=dummyBuf2value();
//        printf("roll [%f]\n",test.yaw);
    test.debugString();
}

INS_STATUS INS570D_Driver::dummyBuf2value()
{
    INS_STATUS *tmp_=&(this->ins570d);
    getRPY(*tmp_);
    getGYRO(*tmp_);
    getACC(*tmp_);
    getWGS84(*tmp_);
    getVEL(*tmp_);
    getALISTATUS(*tmp_);
    getLOOPDATA(*tmp_);
    ins570d=*tmp_;
    return  *tmp_;
}

INS_STATUS &INS570D_Driver::getInsData()
{
    return ins570d;
}

double deg2rad(double deg_){
    return deg_*M_PI/180;
}

void INS570D_Driver::getRPY(INS_STATUS& tmp_)
{
    // roll
    tmp_.roll=deg2rad(hex2Value<uint16_t>(dummy_array_.at(3),ANGLE_RESOLUTION));
    tmp_.pitch=deg2rad(hex2Value<uint16_t>(dummy_array_.at(4),ANGLE_RESOLUTION));
    tmp_.yaw=deg2rad(-hex2Value<uint16_t>(dummy_array_.at(5),ANGLE_RESOLUTION));
    
}

void INS570D_Driver::getGYRO(INS_STATUS &tmp_)
{
    tmp_.gyro_x=hex2Value<uint16_t>(dummy_array_.at(6),GYRO_RESOLUTION);
    tmp_.gyro_y=hex2Value<uint16_t>(dummy_array_.at(7),GYRO_RESOLUTION);
    tmp_.gyro_z=hex2Value<uint16_t>(dummy_array_.at(8),GYRO_RESOLUTION);
}

void INS570D_Driver::getACC(INS_STATUS &tmp_)
{
    tmp_.acc_x=hex2Value<uint16_t>(dummy_array_.at(9),ACC_RESOLUTION);
    tmp_.acc_y=hex2Value<uint16_t>(dummy_array_.at(10),ACC_RESOLUTION);
    tmp_.acc_z=hex2Value<uint16_t>(dummy_array_.at(11),ACC_RESOLUTION);
}

void INS570D_Driver::getWGS84(INS_STATUS &tmp_)
{
    tmp_.latitude=hex2Value<uint32_t>(dummy_array_.at(12),WGS_RESOLUTION);
    tmp_.longitude=hex2Value<uint32_t>(dummy_array_.at(13),WGS_RESOLUTION);
    tmp_.altitude=hex2Value<uint32_t>(dummy_array_.at(14),WGS_RESOLUTION);
}

void INS570D_Driver::getVEL(INS_STATUS &tmp_)
{
    tmp_.vel_n=hex2Value<uint16_t>(dummy_array_.at(15),VEL_RESOLUTION);
    tmp_.vel_e=hex2Value<uint16_t>(dummy_array_.at(16),VEL_RESOLUTION);
    tmp_.vel_d=hex2Value<uint16_t>(dummy_array_.at(17),VEL_RESOLUTION);
}

// 4个bit都为1时，表示初始化对准完成
void INS570D_Driver::getALISTATUS(INS_STATUS &tmp_)
{
    uint8_t status_=(uint8_t)dummy_array_.at(18);
    if(status_==0xFF){
        tmp_.ali=ALIGNED;
    }else{
        tmp_.ali=NOT_ALIGEN;
    }
}

void INS570D_Driver::getLOOPDATA(INS_STATUS &tmp_)
{
    uint8_t type_=(uint8_t)dummy_array_.at(23);
    tmp_.data_type_=(data_type)type_;

    switch ((data_type)type_) {
    case POSI_PRECISION:getPOSI_STD(tmp_);  break;
    case VEL_PRECISION:getVEL_STD(tmp_);    break;
    case POSE_PRECISION:getPOSE_STD(tmp_);  break;
    case TEMPERATURE:getTEMPERTURE(tmp_);   break;
    case GPS_STATUS:getGPSSTATUS(tmp_);     break;
    case WHEEL_SPEED_STATUS:getWHEELSPEEDSTATUS(tmp_);break;
    default:{
        //printf("WRONG Data Type!! Please Check!\n");
        break;
    }
    }
}

void INS570D_Driver::getDATA123(double data123[])
{
    data123[0]=hex2Value<uint16_t>(dummy_array_.at(19),1);
    data123[1]=hex2Value<uint16_t>(dummy_array_.at(20),1);
    data123[2]=hex2Value<uint16_t>(dummy_array_.at(21),1);
}

void INS570D_Driver::getPOSI_STD(INS_STATUS &tmp_)
{
    double data[3];
    getDATA123(data);
    tmp_.lat_std=exp(data[0]/100);
    tmp_.lon_std=exp(data[1]/100);
    tmp_.alti_std=exp(data[2]/100);
}

void INS570D_Driver::getVEL_STD(INS_STATUS &tmp_)
{
    double data[3];
    getDATA123(data);
    tmp_.vn_std=exp(data[0]/100);
    tmp_.ve_std=exp(data[1]/100);
    tmp_.vd_std=exp(data[2]/100);
}

void INS570D_Driver::getPOSE_STD(INS_STATUS &tmp_)
{
    double data[3];
    getDATA123(data);
    tmp_.roll_std=exp(data[0]/100);
    tmp_.pitch_std=exp(data[1]/100);
    tmp_.yaw_std=exp(data[2]/100);
}

void INS570D_Driver::getTEMPERTURE(INS_STATUS &tmp_)
{
    tmp_.dev_temperature=hex2Value<uint16_t>(dummy_array_.at(19),TEMPETURE_RESOLUTION);
}

void INS570D_Driver::getGPSSTATUS(INS_STATUS &tmp_)
{
    double data[3];
    getDATA123(data);
    if(data[0]==NARROW_INT&&data[2]==NARROW_INT){
        tmp_.fix_type=NARROW_INT;
    }else if(data[0]==NARROW_FLOAT&&data[2]==NARROW_FLOAT){
        tmp_.fix_type=NARROW_FLOAT;
    }else{
        tmp_.fix_type=GPS_NONE;
    }
    tmp_.satellites_num=data[1];
}

void INS570D_Driver::getWHEELSPEEDSTATUS(INS_STATUS &tmp_)
{
    tmp_.wheel_speed_status=hex2Value<uint16_t>(dummy_array_.at(20),1);
}


// 设置buffer_
bool INS570D_Driver::setBuffer(const std::vector<uint8_t>&buf)
{
    buffer_=buf;
    if(buffer_.back()!=checksum()){
        //printf("checksum error\n");
        return false;
    }else{
        //printf("checksum OK!\n");
        return true;
        // printf("checksum success [%x] \n",buffer_[0]);
        //  printf("checksum success [%x] [%x] [%x] \n",buffer_[0],buffer_[1],buffer_[2]);
    }
    //printf("%x\n",buffer_[0]);
    return true;
}

// 异或校验
uint8_t INS570D_Driver::checksum()
{
    //    unsigned char i;
    //    unsigned char checksum = 0;

    //    for(i = 0; i < len; i++) {
    //        checksum ^=*buf++;
    //    }

    //    return checksum;

    uint8_t checksum_=0;
    for(size_t i=0;i<buffer_.size()-1;i++){
        checksum_ ^=buffer_[i];
    }
    return checksum_;
}
